
module Geom

    #
    # Describes a quaternion. 
    #
    class Quaternion
        
        EPSILON = 1e-10

        def initialize(*args)
            if (args.length == 1)
                @m_data = args[0]
            end
            @m_data = [args[0], args[1], args[2], args[3]] if (args.length == 4);
        end
        
        def Quaternion.fromTransformation(tr)
            tr.inspect
            
            arr = tr.to_a
            a_matrix = [arr[0..3], arr[4..7], arr[8..11], arr[12..15]];
            m_data = [0,0,0,0];
                scalerow = a_matrix[0][0] + a_matrix[1][1] + a_matrix[2][2];
            
                if(scalerow > 0.0)
                    s = Math.sqrt(scalerow + a_matrix[3][3]);

                    m_data[3] = s * 0.5;

                    s = 0.5 / s;

                    m_data[0] = (a_matrix[1][2] - a_matrix[2][1]) * s;
                    m_data[1] = (a_matrix[2][0] - a_matrix[0][2]) * s;
                    m_data[2] = (a_matrix[0][1] - a_matrix[1][0]) * s;
                else
                    i = 0;
                    if (a_matrix[1][1] > a_matrix[0][0]) then; i = 1; end
                    if (a_matrix[2][2] > a_matrix[i][i]) then; i = 2; end

                    j = (i+1)%3;
                    k = (j+1)%3;
                    
                    s = Math.sqrt((a_matrix[i][i] - (a_matrix[j][j] + a_matrix[k][k])) + a_matrix[3][3]);

                    m_data[i] = s * 0.5;

                    s = 0.5 / s;

                    m_data[3] = (a_matrix[j][k] - a_matrix[k][j]) * s;
                    m_data[j] = (a_matrix[i][j] + a_matrix[j][i]) * s;
                    m_data[k] = (a_matrix[i][k] + a_matrix[k][i]) * s;
                end

                if (a_matrix[3][3] != 1.0)
                    scale = 1.0 / Math.sqrt(a_matrix[3][3]);
                    #operator*=(1.0 / Math.sqrt(a_matrix[3][3]));
                    m_data[0] *= scale;
                    m_data[1] *= scale;
                    m_data[2] *= scale;
                    m_data[3] *= scale;
                end
                
                return Quaternion.new(m_data);
        end
        
        #
        # Constructs a rotation which is the minimum rotation necessary to make the a_rotate_from vector 
        # to point in the direction of the a_rotate_to vector.
        #
        def Quaternion.rotateFromTo(a_rotate_from, a_rotate_to)
            from = a_rotate_from.normalize
            
            to = a_rotate_to.normalize
            
            dot = from.dot(to)
            
            crossvec = from.cross(to)
            
            crosslen = crossvec.length()
            
            if crosslen < EPSILON  # Parallel vectors
                # Check if they are pointing in the same direction.
                if dot > 0.0
                    retval = Quaternion.new(0.0, 0.0, 0.0, 1.0)
                else
                    # Ok, so they are parallel and pointing in the opposite direction of each other.
                    # Try crossing with x axis.
                    t = from.cross(Geom::Vector3d.new(1.0, 0.0, 0.0))
                    # If not ok, cross with y axis.
                    if t.length < EPSILON 
                        t = from.cross(Geom::Vector3d.new(0.0, 1.0, 0.0))
                    end            
                    t.normalize!()
                    retval = Quaternion.new(t[0], t[1], t[2], 0.0)
                end
            else # Vectors are not parallel
                crossvec.normalize!()
                # The fabs() wrapping is to avoid problems when `dot' "overflows"
                # a tiny wee bit, which can lead to sqrt() returning NaN.
                a = Math::sqrt(0.5 * (1.0 - dot).abs())
                crossvec.set!(crossvec.x * a, crossvec.y * a, crossvec.z * a)

                # The fabs() wrapping is to avoid problems when `dot' "underflows"
                # a tiny wee bit, which can lead to sqrt() returning NaN.
                q4 = Math::sqrt(0.5 * (1.0 + dot).abs())
                retval = Quaternion.new(crossvec[0], crossvec[1], crossvec[2], q4)
            end
            return retval.normalize()
        end
        
        def rotate(vec1)
            if vec1.kind_of? Array
                vec1 = Geom::Vector3d.new(vec1)
            end
            tr = getTransformation()
            vec2 = tr * vec1
            return vec2
        end
        
        def normalize()
            len = length()
            if len < EPSILON
                q = Quaternion.new(0, 0, 0, 1)
            else
                q = Quaternion.new(@m_data[0]/len, @m_data[1]/len, @m_data[2]/len, @m_data[3]/len)
            end
            return q
        end
        
        def length()
            return Math::sqrt(@m_data[0]**2 + @m_data[1]**2 + @m_data[2]**2 + @m_data[3]**2)
        end

        def getMatrix()
            matrix=[[],[],[],[]];
            # calculate coefficient
            x2 = @m_data[0] + @m_data[0]; 
            y2 = @m_data[1] + @m_data[1]; 
            z2 = @m_data[2] + @m_data[2];
            xx = @m_data[0] * x2; xy = @m_data[0] * y2; xz = @m_data[0] * z2;
            yy = @m_data[1] * y2; yz = @m_data[1] * z2; zz = @m_data[2] * z2;
            wx = @m_data[3] * x2; wy = @m_data[3] * y2; wz = @m_data[3] * z2;


            matrix[0][0] = 1.0 - (yy + zz); 
            matrix[1][0] = xy - wz;
            matrix[2][0] = xz + wy; 
            matrix[3][0] = 0.0;

            matrix[0][1] = xy + wz; 
            matrix[1][1] = 1.0 - (xx + zz);
            matrix[2][1] = yz - wx; 
            matrix[3][1] = 0.0;


            matrix[0][2] = xz - wy; 
            matrix[1][2] = yz + wx;
            matrix[2][2] = 1.0 - (xx + yy); 
            matrix[3][2] = 0.0;


            matrix[0][3] = 0.0; 
            matrix[1][3] = 0.0;
            matrix[2][3] = 0.0; 
            matrix[3][3] = 1.0;

            return matrix[0]+matrix[1]+matrix[2]+matrix[3];
        end
        
        def getTransformation()
            return Geom::Transformation.new(getMatrix());
        end
        
        # in radians
        def getPhi()
            tr = getTransformation()
            ox = tr * Geom::Vector3d.new(1,0,0);
            return Geom::Vector3d.new(0,0,1).angle_between(ox);
        end
        
        # in radians
        def getTheta()
            tr = getTransformation()
            oz = tr * Geom::Vector3d.new(0,0,1);
            angle = Geom::Vector3d.new(0,1,0).angle_between(oz);
            if oz.x < 0
                angle = -angle
            end
            return -angle - 180.degrees;
        end
        
        def [](i)
            return @m_data[i];
        end
   
   
   
   
        # in radians
        def getPhi2()
            tr = getTransformation()
            ox = tr * Geom::Vector3d.new(1,0,0);
            return Geom::Vector3d.new(0,1,0).angle_between(ox)-90.degrees;
        end
        
        # in radians
        def getTheta2()
            tr = getTransformation()
            oz = tr * Geom::Vector3d.new(0,0,1);
            angle = Geom::Vector3d.new(0,0,1).angle_between(oz);
            if oz.x < 0
                angle = 360.degrees-angle
            end

            return angle;
        end
        
    end

end